#include <opencv2/opencv.hpp>
#include "fiocv.hpp"

static void calib(std::string imagePath, std::string ymlPath, cv::Size sz=cv::Size(7,7))
{
    cv::Mat m=cv::imread(imagePath);

    cv::Mat mg=cv::imread(imagePath,0);

    std::vector<cv::Point2f> center;

    cv::findCirclesGrid(mg,sz,center);

    cv::drawChessboardCorners(m,sz,center,true);

    cv::imshow("m",m);

    cv::waitKey();

    cv::Mat cm=cv::Mat(center).reshape(2,sz.height);

    std::cout<<cm<<"\n";


    float pp[]={
        209.553,
        -82.262,
        //    45.995,
        211.390,
        55.295,
        //    45.995,
        346.636,
        52.407,
        //    45.999,
        344.044,
        -84.108,
        //    45.994, 
    };

    cv::Vec2f *ppp=(cv::Vec2f*)pp;

    std::vector<cv::Vec2f> ppv(ppp,ppp+4);

    std::cout<<"ppv="<<cv::Mat(ppv)<<"\n";

    std::vector<cv::Vec2f> pxl(4);

    pxl[0]=cm.at<cv::Vec2f>(0,0);
    pxl[1]=cm.at<cv::Vec2f>(0,sz.width-1);
    pxl[2]=cm.at<cv::Vec2f>(sz.height-1,sz.width-1);
    pxl[3]=cm.at<cv::Vec2f>(sz.height-1,0);

    std::cout<<"pxl="<<cv::Mat(pxl)<<"\n";

    cv::Mat H=cv::findHomography(pxl,ppv);

    std::cout<<"H="<<H<<"\n";

    Save(H,ymlPath);

}



int main()
{

#if 0
    cv::VideoCapture c(0);

    cv::Mat m;
    int k=0;
    while(k<1000)
    {
        if(c.read(m))
        {
            cv::imshow("m",m);
            cv::waitKey(1);
        }
        k++;
    }
    c.release();

        cv::imwrite("m.png",m);
#endif

        calib("m.png", "H.yml");


    return 0;
}
